using System.Threading;
using System.Threading.Tasks;
using Avalonia;
using Avalonia.Controls;
using Avalonia.Interactivity;
using Avalonia.Markup.Xaml;
using Messages.geometry_msgs;
using RosSharp;

namespace AvaloniaSim.Views
{
    public partial class DrawCircleLocal : Window
    {
        private Publisher<Messages.geometry_msgs.Twist> pub;

        private bool m_HaveInitRos = false;


        public DrawCircleLocal()
        {
            InitializeComponent();
#if DEBUG
            this.AttachDevTools();
#endif
        }

        private void InitializeComponentO()
        {
            AvaloniaXamlLoader.Load(this);
        }

        private void InitRos()
        {
            ROS.ROS_MASTER_URI = $"http://{txtMasterIP.Text}:{txtMasterPort.Text}";

            ROS.ROS_HOSTNAME = txtHostName.Text;
            if (!string.IsNullOrEmpty(txtRosIP.Text))
            {
                ROS.ROS_IP = txtRosIP.Text;
            }



            string nodeName = txtNodeName.Text;
            ROS.Init(nodeName);
            NodeHandle nh = new NodeHandle();
            pub = nh.Advertise<Messages.geometry_msgs.Twist>(txtTopic.Text, 1, false);
            m_HaveInitRos = true;
        }

        private void BtnRosPub_OnClick(object sender, RoutedEventArgs e)
        {

            if (m_HaveInitRos == false)
            {
                InitRos();
            }

            Task.Run(() =>
            {
                while (true)
                {
                    Messages.geometry_msgs.Twist message = new Messages.geometry_msgs.Twist();
                    message.linear = new Vector3();
                    message.angular = new Vector3();
                    message.linear.x = 1;
                    message.linear.y = 0;
                    message.linear.z = 0;
                    message.angular.x = 0;
                    message.angular.y = 0;
                    message.angular.z = 2;
                    pub.Publish(message);
                }
            });

        }
    }
}
